package com.grt192.macro;

import com.googlecode.gunncsnavigation.telemetry.TelemetryPorts;
import com.grt192.actuator.spot.GRTDemoLED;
import com.grt192.controller.spot.TelemetryServer;
import com.grt192.core.CommandArray;
import com.grt192.core.GRTObject;
import com.grt192.core.controller.Macro;
import com.grt192.event.SensorChangeListener;
import com.grt192.event.SensorEvent;
import com.grt192.mechanism.spot.SLight;
import com.grt192.sensor.DemoJoystick;
import com.grt192.utils.Util;

/**
 * A macro which sends Accelerometer tilt data with the TelemetryServer class
 * @author Andrew Chen <andrewtheannihilator@gmail.com>
 */
public class SpotJoyStick extends GRTObject implements Macro,
        SensorChangeListener,
        TelemetryPorts {

    private static final double MAX_G = 1.5;
    //lights
    private SLight[] lights = SLight.get();
    private static int[] accelLights = new int[]{2, 3, 4};
    static final double CHANGE_THRESHOLD = .05;
    //joystick
    private DemoJoystick joy;
    //sensor service
    private TelemetryServer server;

    public SpotJoyStick() {
        setPrinting(true);
    }

    /** Start this macro */
    public int execute() {
        initLights();
        startSensors();

        while (true) {
            Util.sleep(1000);
        }
    }

    /** Waiting status LEDs */
    public void initLights() {
        log("initializing lights...");
        for (int i = 0; i < accelLights.length; i++) {
            lights[accelLights[i]].rawColor(GRTDemoLED.Color.RED);
        }
        log("Lights initialization success!");
    }

    /** Initialize and start sensors */
    public void startSensors() {
        log("Initializing joystick...");
        joy = new DemoJoystick();
        joy.addSensorChangeListener(this);
        joy.start();
        log("Joystick initialization success!");

        log("Initializing Telemetry Server");
        server = new TelemetryServer(joy, JOYSTICK_PORT);
        server.start();
        log("Telemetry server initialization success!");

    }

    /**
     * Converts a number in a known range [inMin... inMax] to a new range [outMin... outMax]
     * @param intMin
     * @param intMax
     * @param outMin
     * @param outMax
     * @param in
     * @return 
     */
    private static double convertPercent(double intMin, double intMax, double outMin, double outMax, double in) {
        /*to get a percent:
         * find out how the number, shifted to zero, compares to the range shifted to zero.
         */
        double percent = (in - intMin) / (intMax - intMin);
        double newValue = (percent * (outMax - outMin)) + outMin;
        return newValue;

    }

    public void sensorStateChanged(SensorEvent e, String key) {
        int c = (int) convertPercent(-MAX_G, MAX_G, 0, 255, e.getData(key));
        CommandArray color = new CommandArray(255 - c, c, 0);
        if (key.equals(DemoJoystick.X)) {
            lights[accelLights[0]].rawColor(color);

        } else if (key.equals(DemoJoystick.Y)) {
            lights[accelLights[1]].rawColor(color);

        } else if (key.equals(DemoJoystick.Z)) {
            lights[accelLights[2]].rawColor(color);

        }
    }
}
